#include "./motors.h"
#include "../xlib/gpio.h"
#include "./sensor.h"
#include "../rtx/rtx_os.h"

namespace cargo_driver {

Motors motors;

namespace motors_impl {

using namespace stm32f1;

using DIR1 = gpio::PC<8>;
using DIR2 = gpio::PC<9>;
using BOX_CTRL = gpio::PC<10>;
using VALVE_CTRL = gpio::PC<11>;
using PUMP_DIR1 = gpio::PC<12>;
using PUMP_DIR2 = gpio::PC<13>;

enum class Dir : uint8_t {
    UP,
    DOWN,
    BRAKE,
};

static void set_dir(Dir dir) {
    switch (dir) {
        case Dir::DOWN: 
            DIR1::set();
            DIR2::clr();
        break;

        case Dir::UP:
            DIR1::clr();
            DIR2::set();
        break;

        case Dir::BRAKE:
            DIR1::clr();
            DIR2::clr();
        break;
    }
}

}

using namespace motors_impl;

void Motors::init() {
    rcc::enable<
        DIR1, DIR2,
        BOX_CTRL, VALVE_CTRL,
        PUMP_DIR1, PUMP_DIR2
    >();

    gpio::config<
        gpio::Cfg<DIR1, gpio::Mode::OUT_50MHz_PP>,
        gpio::Cfg<DIR2, gpio::Mode::OUT_50MHz_PP>,
        gpio::Cfg<BOX_CTRL, gpio::Mode::OUT_50MHz_PP>,
      //  gpio::Cfg<VALVE_CTRL, gpio::Mode::OUT_50MHz_PP>,
        gpio::Cfg<PUMP_DIR1, gpio::Mode::OUT_50MHz_PP>,
        gpio::Cfg<PUMP_DIR2, gpio::Mode::OUT_50MHz_PP>
    >();

    set_dir(Dir::BRAKE);
    BOX_CTRL::clr();
    PUMP_DIR1::clr();
    PUMP_DIR2::clr();
}

Error Motors::box_up() {
    Error err;
    uint32_t time_count = 0;
    
    if (sensor->is_pick_box_up()) {
        goto finish;
    }
    set_dir(Dir::UP);
    BOX_CTRL::set();
    for (;;) {
        if (sensor->is_pick_box_up()) {
            break;
        }

        osDelay(50);
        time_count += 50;
        
        if (time_count >= BOX_MOTOR_TIMEOUT) {
            err = ec::cargo::BOX_MOTOR_TIMEOUT;
            break;
        }
    }
finish:
    set_dir(Dir::BRAKE);
    BOX_CTRL::clr();
    return err;
}

Error Motors::box_down() {
    Error err;
    uint32_t time_count = 0;
    
    if (sensor->is_pick_box_down()) {
        goto finish;
    }
    set_dir(Dir::DOWN);
    BOX_CTRL::set();
    for (;;) {
        if (sensor->is_pick_box_down()) {
            break;
        }

        osDelay(50);
        time_count += 50;
        
        if (time_count >= BOX_MOTOR_TIMEOUT) {
            err = ec::cargo::BOX_MOTOR_TIMEOUT;
            break;
        }
    }
finish:
    set_dir(Dir::BRAKE);
    BOX_CTRL::clr();
    return err;
}

void Motors::comp_open() {
    PUMP_DIR1::clr();
    PUMP_DIR2::set();
}

void Motors::comp_close() {
    PUMP_DIR1::clr();
    PUMP_DIR2::clr();
}

}

